﻿//
//  Contact.cpp
//
#include "Contact.h"

/*
====================================================
ResolveContact
====================================================
*/
// // 只考虑冲量的实现
// void ResolveContact( contact_t & contact ) {
// 	Body* bodyA = contact.bodyA;
// 	Body* bodyB = contact.bodyB;
//
// 	const float invMassA = bodyA-> m_invMass;
// 	const float invMassB = bodyB-> m_invMass;
//
// 	const float elasticityA = bodyA-> m_elasticity;
// 	const float elasticityB = bodyB-> m_elasticity;
// 	const float elasticity = elasticityA * elasticityB;		// "看起来对的,他就是对的",一种很合理的近似,想想金属和黏土的碰撞
// 	
// 	// Calculate the collision impulse
// 	const Vec3 & n = contact.normal;
// 	const Vec3 vab = bodyA-> m_linearVelocity - bodyB-> m_linearVelocity;
// 	const float ImpulseJ = -(1.0f + elasticity) * vab.Dot(n) / (invMassA + invMassB);
// 	const Vec3 vectorImpulseJ = n * ImpulseJ;
// 	bodyA-> ApplyImpulseLinear(vectorImpulseJ * 1.0f);
// 	bodyB-> ApplyImpulseLinear(vectorImpulseJ * -1.0f);
//
// 	// Let’s also move our colliding objects to just outside of each other
// 	const float tA = bodyA-> m_invMass / (bodyA-> m_invMass + bodyB-> m_invMass);
// 	const float tB = bodyB-> m_invMass / (bodyA-> m_invMass + bodyB-> m_invMass);
//
// 	// 注意ds的方向和 bodyA-> m_position - bodyB-> m_position 是一致的
// 	const Vec3 ds = contact.ptOnB_WorldSpace - contact.ptOnA_WorldSpace;
// 	bodyA-> m_position += ds * tA;
// 	bodyB-> m_position -= ds * tB;
// }

// 考虑通用冲量(即考虑了旋转的情况)
// 其实刚体就是两个物体,碰撞的时候碰撞点已知的情况下,根据 "动量守恒" 和“角动量守恒”
// 能推出我们的公式，用来算出碰撞产生的冲量,然后根据冲量来更新两个body的 位置和旋转
void ResolveContact(contact_t& contact) {
	Body* bodyA = contact.bodyA;
	Body* bodyB = contact.bodyB;

	const Vec3 ptOnA = contact.ptOnA_WorldSpace;
	const Vec3 ptOnB = contact.ptOnB_WorldSpace;

	const float elasticityA = bodyA->m_elasticity;
	const float elasticityB = bodyB->m_elasticity;
	const float elasticity = elasticityA * elasticityB;

	const float invMassA = bodyA-> m_invMass;
	const float invMassB = bodyB-> m_invMass;

	const Mat3 invWorldInertiaA = bodyA-> GetInverseInertiaTensorWorldSpace();
	const Mat3 invWorldInertiaB = bodyB-> GetInverseInertiaTensorWorldSpace();

	const Vec3 n = contact.normal;

	const Vec3 ra = ptOnA - bodyA-> GetCenterOfMassWorldSpace();
	const Vec3 rb = ptOnB - bodyB-> GetCenterOfMassWorldSpace();

	const Vec3 angularJA = (invWorldInertiaA * ra.Cross(n)).Cross(ra);
	const Vec3 angularJB = (invWorldInertiaB * rb.Cross(n)).Cross(rb);
	const float angularFactor = (angularJA + angularJB).Dot(n);
	// Get the world space velocity of the motion and rotation
	const Vec3 velA = bodyA-> m_linearVelocity + bodyA-> m_angularVelocity.Cross(ra);
	const Vec3 velB = bodyB-> m_linearVelocity + bodyB-> m_angularVelocity.Cross(rb);
	// Calculate the collision impulse
	const Vec3 vab = velA - velB;
	const float ImpulseJ = (1.0f + elasticity) * vab.Dot(n) / (invMassA + invMassB + angularFactor);
	const Vec3 vectorImpulseJ = n * ImpulseJ;
	bodyA-> ApplyImpulse(ptOnA, vectorImpulseJ * -1.0f);
	bodyB-> ApplyImpulse(ptOnB, vectorImpulseJ * 1.0f);

	//
	// Calculate the impulse caused by friction
	//
	const float frictionA = bodyA->m_friction;
	const float frictionB = bodyB->m_friction;
	const float friction = frictionA * frictionB;
	// Find the normal direction of the velocity with respect to the normal of the collision
	const Vec3 velNorm = n * n.Dot(vab);
	// Find the tangent direction of the velocity with respect to the normal of the collision
	const Vec3 velTang = vab - velNorm;
	// Get the tangential velocities relative to the other body
	Vec3 relativeVelTang = velTang;
	relativeVelTang.Normalize();
	const Vec3 inertiaA = (invWorldInertiaA * ra.Cross(relativeVelTang)).Cross(ra);
	const Vec3 inertiaB = (invWorldInertiaB * rb.Cross(relativeVelTang)).Cross(rb);
	const float invInertia = (inertiaA+inertiaB).Dot(relativeVelTang);
	// Calculate the tangential impulse for friction
	const float reducedMass = 1.0f / (bodyA->m_invMass + bodyB->m_invMass + invInertia);
	const Vec3 impulseFriction = velTang * reducedMass * friction;
	// Apply kinetic friction
	bodyA->ApplyImpulse(ptOnA, impulseFriction * -1.0f);
	bodyB->ApplyImpulse(ptOnB, impulseFriction * 1.0f);
	
	//
	// Let's also move our colliding objects to just outside of each other
	//
	const float tA = bodyA-> m_invMass / (bodyA-> m_invMass + bodyB-> m_invMass);
	const float tB = bodyB-> m_invMass / (bodyA-> m_invMass + bodyB-> m_invMass);
	const Vec3 ds = contact.ptOnB_WorldSpace - contact.ptOnA_WorldSpace;
	bodyA-> m_position += ds * tA;
	bodyB-> m_position -= ds * tB;
}
